function [c_new]=update_att(c_before,v_ins,p_ins,up_angle,ts)
%%
%用于姿态更新
%输入：原始矩阵c_before
%      初始速度v_ins
%      初始位置p_ins
%      陀螺仪输出up_angle
%      采样频率ts
%输出：新姿态矩阵c_new
%%
% 参数引入
%地球参数设置
v_ie=7.292e-5; %地球自转角速率
a=6378254;     %地球长半径
e=1/298.3;     %偏心率
%g=9.8;

%速度引入
v_e=v_ins(1);
v_n=v_ins(2);
%v_u=v_ins(3);

%位置引入
L_new=p_ins(1);
%la_new=p_ins(2);
h_new=p_ins(3);

%地球参数引入
R_m=a/sqrt(1-e^2*sin(L_new));
R_n=a*(1-e^2)/sqrt((1-e^2*sin(L_new)^2)^3);

R_mh=R_m+h_new;
R_nh=R_n+h_new;

%陀螺仪输出
angle_ts_begin=1/2*ts*up_angle;
angle_ts_half=1/2*ts*up_angle;
angle_ts_end=1/2*ts*up_angle;
%%
% 开始姿态更新

%将欧拉角转化成矩阵方便计算
%c_before=euler2m(old_att);


%第一个矩阵
w_ie=[0;v_ie*cos(L_new);v_ie*sin(L_new)];
w_en=[-v_n/R_mh;v_e/R_nh;v_e/R_nh*tan(L_new)];

w_in=w_ie+w_en;
erv_in=ts*w_in;

c_n=mrv(erv_in);
c_n=c_n';

%第二个矩阵
up_m1=angle_ts_half-angle_ts_begin;
up_m2=angle_ts_end-angle_ts_half;
%up_m_sum=up_m1+up_m2;
erv_ib=(up_m1+up_m2)+cross(2/3*up_m1,up_m2);

c_b=mrv(erv_ib);

%姿态更新方程
c_new=c_n*c_before*c_b;

%将矩阵转化成欧拉角方便画图
%new_att=m2euler(c_new);
end